import rospy
from std_msgs.msg import Int32
from mavros_msgs.msg import PositionTarget
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import numpy as np

class Path_trans:
    def __init__(self):

        rospy.init_node('path_trans')
        self.cmd_pose_array = []
        self.cmd_pose = np.zeros(4)
        self.pose = np.zeros(3)
        self.pub_pose = PositionTarget() # will be published

        self.flag = 0
        self.index = 0
        self.cmd_pose_num = 0

        self.cmd_pose_sub = rospy.Subscriber('/uav1/mavros/setpoint_raw/local',PositionTarget,self.cmd_pose_cb)
        self.pose_sub = rospy.Subscriber('/ugv1/pose',PoseStamped,self.pose_cb)
        self.odom_sub = rospy.Subscriber('/robot1/Odometry',Odometry,self.odom_cb)
        self.flag_sub = rospy.Subscriber('/plan_finished',Int32,self.flag_cb)

        self.cmd_pose_pub = rospy.Publisher('/pose_cmd',PositionTarget,queue_size=10)

        self.rate = rospy.Rate(30)

    def pose_cb(self,data):
        self.pose = [data.pose.position.x,
                     data.pose.position.y,
                     data.pose.position.z]
        
    def odom_cb(self,data):
        self.pose = [data.pose.pose.position.x,
                     data.pose.pose.position.y,
                     data.pose.pose.position.z]
        

    def cmd_pose_cb(self,data):
        self.cmd_pose = [data.position.x,
                     data.position.y,
                     data.position.z,
                     data.yaw]
        self.cmd_pose_array.append(self.cmd_pose)
        self.cmd_pose_num = len(self.cmd_pose_array)

    def flag_cb(self,data):
        self.flag = data.data
        print("replan complete, status:",self.flag,"update pose array")
        self.index = 0
        self.cmd_pose_array.clear()
        self.cmd_pose_array.append(self.cmd_pose)

    def main(self):
        while not rospy.is_shutdown():
            # print(self.index," ",len(self.cmd_pose_array))
            if(self.index < len(self.cmd_pose_array)):
                print(self.index," ",len(self.cmd_pose_array))
                print(self.pose[0]," ",self.cmd_pose_array[self.index][0])
                self.pub_pose.position.x = self.cmd_pose_array[self.index][0]
                self.pub_pose.position.y = self.cmd_pose_array[self.index][1]
                self.pub_pose.position.z = self.cmd_pose_array[self.index][2]
                self.pub_pose.yaw = self.cmd_pose_array[self.index][3]

                if(pow(self.cmd_pose_array[self.index][0] - self.pose[0],2)
                + pow(self.cmd_pose_array[self.index][1] - self.pose[1],2) < 0.01):
                    self.index += 1

                self.cmd_pose_pub.publish(self.pub_pose)
            # self.rate.sleep()

if __name__ == "__main__":
    path_trans = Path_trans()
    path_trans.main()
    rospy.spin()

    

        